//合并每一帧的激光点云数据,构成GT激光点云地图.
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>	//	pcl::transformPointCloud 用到这个头文件
#include <fstream>
#include <sstream>


#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

string GT_path = "/home/benoxo/data_odometry_poses/dataset/poses/00.txt";
string LidarCloud_path = "/home/benoxo/2011_10_03/2011_10_03_drive_0027_sync/velodyne_points/data/";
string out_path = "/home/benoxo/kitti_odom_00/";
string label_path = "/home/benoxo/kitti00label/00/labels/";



int main(int argc, char *argv[])
{
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);//世界lidar cloud
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloudS(new pcl::PointCloud<pcl::PointXYZI>);//世界lidar cloud

    ifstream GT; //读取ground truth的数值,一行12个值,组成3*4矩阵;拼接上0001后得4*4的变换矩阵
    std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> vT_c2w;

    GT.open(GT_path);
    while (!GT.eof()){
        Eigen::Matrix4d mT_c2w = Eigen::Matrix4d::Zero(); //camera to world ,store ground truth
        string s;
        getline(GT,s);
        double gt11=0,gt12=0,gt13=0,gt14=0,gt21=0,gt22=0,gt23=0,gt24=0,gt31=0,gt32=0,gt33=0,gt34=0;
        if(!s.empty()){
            stringstream ss;
            ss << s;

            ss >> gt11;
            ss >> gt12;
            ss >> gt13;
            ss >> gt14;

            ss >> gt21;
            ss >> gt22;
            ss >> gt23;
            ss >> gt24;

            ss >> gt31;
            ss >> gt32;
            ss >> gt33;
            ss >> gt34;

            mT_c2w << gt11, gt12, gt13, gt14, gt21, gt22, gt23, gt24, gt31, gt32, gt33, gt34, 0, 0, 0, 1;

        }
        vT_c2w.push_back(mT_c2w);

    }
    // for (int i=0; i<4; ++i){
    //     for (int j=0; j<4; ++j){
    //         cout << vT_c2w[1100](i,j) << "\t";
    //     }
    //     cout << endl;
    // }
    // cout << vT_c2w.size() << endl;

    Eigen::Matrix4d mT_l2c = Eigen::Matrix4d::Zero(); //T_l2c矩阵,lidar to camera的变换矩阵,外参
    mT_l2c << 7.027555e-03, -9.999753e-01, 2.599616e-05, -7.137748e-03,
            -2.254837e-03, -4.184312e-05, -9.999975e-01, -7.482656e-02,
            9.999728e-01, 7.027479e-03, -2.255075e-03, -3.336324e-01,
            0,0,0,1;

    // Eigen::Matrix4d mT = Eigen::Matrix4d::Zero();
    // mT = vT_c2w[1] * mT_l2c;

    for (int k=1; k <= 4540; k+=4){ //遍历每一个pcd文件,对其做变换,转到世界坐标系下.

        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_k(new pcl::PointCloud<pcl::PointXYZI>);//每一帧的lidar cloud
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_k_transformed(new pcl::PointCloud<pcl::PointXYZI>);//每一帧的lidar cloud
        pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>());

        stringstream ss;
        ss << LidarCloud_path << "/";
        ss << setw(10) << setfill('0') << k;
        ss << ".bin";
        string LidarCloud_path_k = ss.str();

        fstream data_io_(LidarCloud_path_k, std::ios::binary | std::ios::in | std::ios::out);
        while (!data_io_.eof()) {
            float x, y, z, i;
            data_io_.read(reinterpret_cast<char *>(&x), sizeof(x));
            data_io_.read(reinterpret_cast<char *>(&y), sizeof(y));
            data_io_.read(reinterpret_cast<char *>(&z), sizeof(z));
            data_io_.read(reinterpret_cast<char *>(&i), sizeof(i));

            pcl::PointXYZI point;
            point.x = x;
            point.y = y;
            point.z = z;
            cloud_k->push_back(point);
        }

        Eigen::Matrix4d mT_final = vT_c2w[k] * mT_l2c;
        Eigen::Affine3d aT_final(mT_final);
        std::cout << to_string(k) << std::endl;
        pcl::transformPointCloud (*cloud_k, *cloud_k_transformed, aT_final);

        /*pcl::VoxelGrid<pcl::PointXYZI> sor;   // 创建滤波器对象
        sor.setInputCloud (cloud_k_transformed);   //给滤波器对象设置需要过滤的点云
        sor.setLeafSize (0.2f, 0.2f, 0.2f); //设置滤波时创建的体素大小为1cm立方体
        sor.filter (*cloud_filtered); //执行滤波处理

	pcl::VoxelGrid<pcl::PointXYZI> sor;   // 创建滤波器对象
        sor.setInputCloud (cloud_k_transformed);   //给滤波器对象设置需要过滤的点云
        sor.setLeafSize (0.1f, 0.1f, 0.1f); //设置滤波时创建的体素大小为1cm立方体
        sor.filter (*cloud_filtered); //执行滤波处理
	*/


        //*cloud = *cloud + *cloud_k_transformed;
        //*cloudS = *cloudS + *cloud_filtered;


        stringstream label_ss;
        label_ss << label_path << "/";
        label_ss << setw(6) << setfill('0') << k;
        label_ss << ".label";
        string cur_label_path = label_ss.str();
        fstream label_io(cur_label_path, std::ios::binary | std::ios::in);
        fstream out_io(out_path + "/" + to_string(k) + ".bin", std::ios::binary | std::ios::app | std::ios::out);
        for (int i = 0; i < cloud_k_transformed->size(); i++) {
            auto& point = cloud_k_transformed->at(i);
            out_io.write(reinterpret_cast<char *>(&point.x), sizeof(point.x));
            out_io.write(reinterpret_cast<char *>(&point.y), sizeof(point.y));
            out_io.write(reinterpret_cast<char *>(&point.z), sizeof(point.z));
            uint16_t cls = 0;
            uint16_t ints = 0;
            label_io.read(reinterpret_cast<char *>(&cls), sizeof(cls));
            label_io.read(reinterpret_cast<char *>(&ints), sizeof(ints));
            out_io.write(reinterpret_cast<char *>(&cls), sizeof(cls));
            out_io.write(reinterpret_cast<char *>(&ints), sizeof(ints));
            //std::cout << cls << std::endl;
            //std::cout << ints << std::endl;
            std::cout << cur_label_path << std::endl;
        }
        out_io.close();

    }
    //pcl::PCDWriter writer;
    // Save DoN features
    //writer.write<pcl::PointXYZI> (out_path, *cloud, false);
    //pcl::io::savePCDFileBinary(out_path + "all.pcd", *cloud);
    //pcl::io::savePCDFileBinary(out_path + "s.pcd", *cloudS);
    return 0;
}
